#include "Transform.h"
#include "FastOps.h"

using namespace std;
using namespace RSIM;

////////////////////////////////////////////////////////////////////////

TransformBase::TransformBase():R_(),P_(0.0){
	R_.Identity();
}

////////////////////////////////////////////////////////////////////////

TransformBase::TransformBase(const Vect3& V):R_(),P_(V){
	R_.Identity();
}

////////////////////////////////////////////////////////////////////////

TransformBase::TransformBase(const Mat3x3& RotMat):R_(RotMat),P_(0.0){
}

////////////////////////////////////////////////////////////////////////

TransformBase::TransformBase(const Mat3x3& R, const Vect3& P):R_(R),P_(P){}

////////////////////////////////////////////////////////////////////////

void TransformBase::premultiply(const Rotation& RotMat){
	double tmp[9];
	FastOps::mult_mat33_mat33(RotMat.Ptr(),R_.Ptr(),tmp);
	R_.copy(tmp);
	
	FastOps::mult_mat33_vec3(RotMat.Ptr(),P_.Ptr(),tmp);
	P_.copy(tmp);
}

////////////////////////////////////////////////////////////////////////

void TransformBase::operator=(const TransformBase& T){
	R_.copy(T.R().Ptr()); 
	P_.copy(T.P().Ptr());
}

////////////////////////////////////////////////////////////////////////

void TransformBase::setR(const Mat3x3& RotMat){
	R_.copy(RotMat.Ptr());
}

////////////////////////////////////////////////////////////////////////

void TransformBase::setP(const Vect3& V){
	P_.copy(V.Ptr());
}

////////////////////////////////////////////////////////////////////////

Transform::Transform():TransformBase(){}

////////////////////////////////////////////////////////////////////////

Transform::Transform(const Vect3& V):TransformBase(V){}

////////////////////////////////////////////////////////////////////////

Transform::Transform(const Mat3x3& RotMat):TransformBase(RotMat){}

////////////////////////////////////////////////////////////////////////

Transform::Transform(const Mat3x3& R, const Vect3& P):TransformBase(R,P){}

////////////////////////////////////////////////////////////////////////

Transform::Transform(const Transform& T):TransformBase(T.R(),T.P()){}

////////////////////////////////////////////////////////////////////////

InvTransform::InvTransform(const Transform& T):TransformBase(Mat3x3(~T.R()),Vect3((~T.R())*(T.P()*(-1)))){ // sFlag
} 

////////////////////////////////////////////////////////////////////////

std::ostream& operator<<(ostream& func, const TransformBase& T){
	func<<"\nPosition Vector: "<<T.P()<<endl;
	func<<"Rotation Matrix\n"<<T.R()<<endl;
	return func;
}

////////////////////////////////////////////////////////////////////////